//
// Created by HP on 2024/1/25.
//

#include "as5047_driver.h"


float speed_buf[10] = {0};
float speed_sum = 0;
/*spi句柄,cs端口,cs管角*/
AS5047_Driver::AS5047_Driver(SPI_HandleTypeDef *spi,GPIO_TypeDef* CS_Porti,uint16_t CS_Pini){
    hspi = spi;
    CS_Port = CS_Porti;
    CS_Pin  = CS_Pini;
    _spi_cs_H();
    cnt = 0;
    speed_rad = 0;
    pos_gap   = 0;
}


void AS5047_Driver::_spi_cs_H(){
    HAL_GPIO_WritePin(CS_Port, CS_Pin, GPIO_PIN_SET);
}

void AS5047_Driver::_spi_cs_L(){
    HAL_GPIO_WritePin(CS_Port, CS_Pin, GPIO_PIN_RESET);
}

void AS5047_Driver::_spi_mode_set(){
//    if(hspi->Init.CLKPhase == SPI_PHASE_2EDGE){
//        return;
//    };

    if (HAL_SPI_DeInit(hspi) != HAL_OK)
    {
        while( true )
            usb_printf("as5047p spi deinit error\r\n");
    }

    hspi->Instance = hspi->Instance;
    hspi->Init.Mode = SPI_MODE_MASTER;
    hspi->Init.Direction = SPI_DIRECTION_2LINES;
    hspi->Init.DataSize = SPI_DATASIZE_16BIT;
    hspi->Init.CLKPolarity = SPI_POLARITY_LOW;
    hspi->Init.CLKPhase = SPI_PHASE_2EDGE;
    hspi->Init.NSS = SPI_NSS_SOFT;
    hspi->Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_4;
    hspi->Init.FirstBit = SPI_FIRSTBIT_MSB;
    hspi->Init.TIMode = SPI_TIMODE_DISABLE;
    hspi->Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
    hspi->Init.CRCPolynomial = 10;
    if (HAL_SPI_Init(hspi) != HAL_OK)
    {
        while( true )
            usb_printf("as5047p spi deinit error\r\n");
    }

}

void AS5047_Driver::_spi_send(uint16_t addr,uint16_t data){
    uint8_t addrs[2];
    uint8_t datas[2];

    addrs[0] = addr >> 8;
    addrs[1] = addr & 0x00ff;
    datas[0] = data >> 8;
    datas[1] = data & 0x00ff;
    _spi_cs_L();
    HAL_SPI_Transmit(hspi,addrs,2,0xffff);
    _spi_cs_H();
    _spi_cs_L();
    HAL_SPI_Transmit(hspi,datas,2,0xffff);
    _spi_cs_H();
}

uint16_t __attribute__((optimize("O0"))) AS5047_Driver::_spi_read(uint16_t addr){
    volatile uint8_t delay = 20;
    uint16_t data;
    _spi_cs_L();
    HAL_SPI_TransmitReceive(hspi,(uint8_t *)&addr,(uint8_t *)&data,1,100);
    _spi_cs_H();
    addr = 0xC000;
    while(delay--);
    _spi_cs_L();
    HAL_SPI_TransmitReceive(hspi,(uint8_t *)&addr,(uint8_t *)&data,1,100);
    _spi_cs_H();
    return data;
}

/*dynamic angle error compensation*/
float __attribute__((optimize("O0"))) AS5047_Driver::as5047_read_angle(bool daec_en){
    float angle;
    uint16_t data;
    uint16_t addr;
    _spi_mode_set(); //设置spi的模式为as5047的模式
AS5047_RE_READ:
    while( true ){
        addr = daec_en ? 0xffff : 0x7ffe;
        data = _spi_read(addr);
        if( (data & 0x4000) != 0){
            uint16_t error = _spi_read(0x4001);
            usb_printf("AS5047 READ error %x\r\n",error);
            HAL_Delay(10);
        }else{
            break;
        }
    }
    uint8_t i,num = 0;
    for(i=0;i<15;i++){
        if((data >> i) & 1)
            num += 1;
    }
    if((((num & 0x01) == 1) && ((data >> 15) == 0)) || ( (num & 0x01) == 0) && ((data >> 15) == 1)){
        usb_printf("AS5047 READ Data Parity Bit Error %d\r\n",data);
        HAL_Delay(10);
        goto AS5047_RE_READ;
    }
    data = (data & 0x3fff);
    angle =  (float)(data*360) / 16384.0f;

    cnt +=1;
    if( cnt == 5) {
        float angle_error = (angle - last_angle);
        if (angle_error > 240)
            angle_error = angle_error - 360;
        else if(angle_error < -240)
            angle_error = angle_error + 360;
        speed_rad = ((angle_error) * 0.017453292f)   * 1000; //弧度  speed_rad * 0.2f + 0.8f *
        pos_gap = angle_error;
        speed_sum -= speed_buf[0];
        for(i=0;i < 9;i++){
            speed_buf[i] = speed_buf[i+1];
        }
        speed_buf[9] = speed_rad;
        speed_sum += speed_buf[9];
        speed_rad = speed_sum / 10.0f;
        last_angle = angle;
        cnt = 0;
        speed_update = 1;
    }
    return angle;
}

//位置偏差
float AS5047_Driver::as5047_read_gap(){
    return pos_gap;
}
//获取速度
float AS5047_Driver::as5047_read_speed(){
    speed_update = 0;
    return speed_rad;
}